#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>

// 定义 IMU 数据结构
typedef struct {
    double timestamp;        // 时间戳，单位：秒
    double accel_x;          // 加速度计 X 轴，单位：m/s²
    double accel_y;          // 加速度计 Y 轴，单位：m/s²
    double accel_z;          // 加速度计 Z 轴，单位：m/s²
    double gyro_x;           // 陀螺仪 X 轴，单位：rad/s
    double gyro_y;           // 陀螺仪 Y 轴，单位：rad/s
    double gyro_z;           // 陀螺仪 Z 轴，单位：rad/s
} ImuData;

// 生成当前时间戳（以秒为单位）
double get_current_timestamp() {
    clock_t current_time = clock();
    return (double)current_time / CLOCKS_PER_SEC;
}

// 模拟读取 IMU 数据（实际应用中这里应该从传感器或文件读取）
ImuData read_imu_data() {
    ImuData data;
    data.timestamp = get_current_timestamp();
    
    // 这里模拟生成随机数据，实际应用中应该从传感器读取
    // 加速度计数据范围通常在 ±2g, ±4g, ±8g 或 ±16g 之间
    data.accel_x = ((double)rand() / RAND_MAX) * 4.0 - 2.0; // -2g 到 2g
    data.accel_y = ((double)rand() / RAND_MAX) * 4.0 - 2.0;
    data.accel_z = ((double)rand() / RAND_MAX) * 4.0 - 2.0;
    
    // 陀螺仪数据范围通常在 ±250°/s, ±500°/s, ±1000°/s 或 ±2000°/s 之间
    // 这里转换为弧度每秒 (rad/s)
    double deg_to_rad = 0.0174533;
    data.gyro_x = (((double)rand() / RAND_MAX) * 500.0 - 250.0) * deg_to_rad; // -250°/s 到 250°/s
    data.gyro_y = (((double)rand() / RAND_MAX) * 500.0 - 250.0) * deg_to_rad;
    data.gyro_z = (((double)rand() / RAND_MAX) * 500.0 - 250.0) * deg_to_rad;
    
    return data;
}

// 将 IMU 数据写入 CSV 文件
int write_imu_data_to_csv(const char *filename, ImuData *data, int num_samples) {
    FILE *file = fopen(filename, "w");
    if (file == NULL) {
        printf("Failed to open file %s for writing\n", filename);
        return -1;
    }
    
    // 写入 CSV 标题行
    fprintf(file, "时间戳(秒),加速度计X(m/s²),加速度计Y(m/s²),加速度计Z(m/s²),陀螺仪X(rad/s),陀螺仪Y(rad/s),陀螺仪Z(rad/s)\n");
    
    // 写入数据
    for (int i = 0; i < num_samples; i++) {
        fprintf(file, "%lf,%lf,%lf,%lf,%lf,%lf,%lf\n", 
                data[i].timestamp, 
                data[i].accel_x, data[i].accel_y, data[i].accel_z, 
                data[i].gyro_x, data[i].gyro_y, data[i].gyro_z);
    }
    
    fclose(file);
    return 0;
}

// 从 CSV 文件读取 IMU 数据
int read_imu_data_from_csv(const char *filename, ImuData **data, int *num_samples) {
    FILE *file = fopen(filename, "r");
    if (file == NULL) {
        printf("Failed to open file %s for reading\n", filename);
        return -1;
    }
    
    // 跳过标题行
    char buffer[256];
    if (fgets(buffer, sizeof(buffer), file) == NULL) {
        fclose(file);
        return -1;
    }
    
    // 计算文件中的数据行数
    *num_samples = 0;
    while (fgets(buffer, sizeof(buffer), file) != NULL) {
        (*num_samples)++;
    }
    
    // 重新设置文件指针到数据开始位置
    rewind(file);
    fgets(buffer, sizeof(buffer), file); // 再次跳过标题行
    
    // 分配内存
    *data = (ImuData *)malloc(sizeof(ImuData) * (*num_samples));
    if (*data == NULL) {
        fclose(file);
        return -1;
    }
    
    // 读取数据
    for (int i = 0; i < *num_samples; i++) {
        fscanf(file, "%lf,%lf,%lf,%lf,%lf,%lf,%lf", 
               &((*data)[i].timestamp), 
               &((*data)[i].accel_x), &((*data)[i].accel_y), &((*data)[i].accel_z), 
               &((*data)[i].gyro_x), &((*data)[i].gyro_y), &((*data)[i].gyro_z));
    }
    
    fclose(file);
    return 0;
}

int main() {
    // 设置随机数种子
    srand((unsigned int)time(NULL));
    
    // 生成示例 IMU 数据
    const int NUM_SAMPLES = 100;
    ImuData *imu_data = (ImuData *)malloc(sizeof(ImuData) * NUM_SAMPLES);
    
    if (imu_data == NULL) {
        printf("Memory allocation failed\n");
        return 1;
    }
    
    printf("Generating %d IMU data samples...\n", NUM_SAMPLES);
    for (int i = 0; i < NUM_SAMPLES; i++) {
        imu_data[i] = read_imu_data();
        // 稍微延迟以模拟实际的数据采集间隔
        for (volatile int j = 0; j < 100000; j++);
    }
    
    // 将数据写入 CSV 文件
    const char *csv_filename = "imu_data.csv";
    if (write_imu_data_to_csv(csv_filename, imu_data, NUM_SAMPLES) == 0) {
        printf("IMU data has been successfully written to file %s\n", csv_filename);
    }
    
    // 从 CSV 文件读取数据
    ImuData *read_data;
    int read_samples;
    if (read_imu_data_from_csv(csv_filename, &read_data, &read_samples) == 0) {
        printf("Successfully read %d samples from %s\n", read_samples, csv_filename);
        
        // 打印前5个样本以验证
        printf("\nFirst 5 sample data:\n");
        printf("Timestamp\t\tAccel_X\t\tAccel_Y\t\tAccel_Z\t\tGyro_X\t\tGyro_Y\t\tGyro_Z\n");
        for (int i = 0; i < (read_samples < 5 ? read_samples : 5); i++) {
            printf("%.6lf\t%.6lf\t%.6lf\t%.6lf\t%.6lf\t%.6lf\t%.6lf\n",
                   read_data[i].timestamp,
                   read_data[i].accel_x, read_data[i].accel_y, read_data[i].accel_z,
                   read_data[i].gyro_x, read_data[i].gyro_y, read_data[i].gyro_z);
        }
        
        free(read_data);
    }
    
    free(imu_data);
    
    return 0;
}